#include "carInParkProc.h"

#include <opencv2/dnn.hpp>
#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"

#include <vector>
#include "yolo.h"

#include "spdlog/spdlog.h"
#include "spdlog/sinks/basic_file_sink.h"

using namespace std;
using namespace cv;

extern std::shared_ptr<spdlog::logger> kylog;

static bool IsRectIntersection(cv::RotatedRect &rRect1, cv::Rect &rect)
{
    bool ret = true;

    std::vector<cv::Point2f> intersectingRegion;
    cv::RotatedRect rRect2(cv::Point2f((rect.x + rect.width / 2), (rect.y / rect.height / 2)), 
    cv::Size2f(rect.width, rect.height), 0);

    cv::Point p1(rect.x, rect.y); 
    cv::Point p2(rect.x + rect.width, rect.y);
    cv::Point p3(rect.x + rect.width, rect.y + rect.height);
    cv::Point p4(rect.x, rect.y + rect.height);

    std::vector<cv::Point> selectpoint;
    selectpoint.push_back(p1);
    selectpoint.push_back(p2);
    selectpoint.push_back(p3);
    selectpoint.push_back(p4);
    
    cv::RotatedRect rotate_rect = cv::minAreaRect(selectpoint);
    
    cv::rotatedRectangleIntersection(rRect1, rotate_rect, intersectingRegion);
    
    if(intersectingRegion.empty()) {
        ret = false;
    }

    return ret; 
}

static bool isCarNotInPark(cv::Rect &carInPark, float carInParkprob, std::vector<YoloBox> &boxes)
{
    bool bret = false;

    for(auto box : boxes) 
    {
        if(box.id == 2) {
            cv::Rect car(box.x, box.y, box.w, box.h);

            cv::Rect tmp = carInPark & car;
            if((1.0f * tmp.area() / carInPark.area() > 0.8f) && carInParkprob < box.prob) 
            {
                //kylog->info("carInParkprob = {}, car prob = {}\r\n", carInParkprob, box.prob);
                bret = true;
                break;
            }
        }
    }

    return bret;
}

static bool checkRaoLu(cv::Rect &carInPark, std::vector<cv::Rect> &cars)
{
    bool bret = false;

    for(auto car : cars) 
    {
        int upRange = car.y + car.height / 2 + 100;
        int downRange = car.y + car.height / 2 - 100;
        int middle = carInPark.y + carInPark.height / 2;

        if(middle > downRange && middle < upRange && (carInPark.x + carInPark.width > car.x + car.width)) {
            bret = true;
            break;
        }
    }

    return bret;
}

static bool checkBigCarWithEmptyPark(std::vector<YoloBox> &boxes, cv::Rect &resultRect)
{
    bool bret = true;

    for(auto box : boxes) {
        //empty park
        if(box.id == 3) {
            cv::Rect boxRect(box.x, box.y, box.w, box.h);
            cv::Rect tmp = boxRect & resultRect;
            if(1.0f * tmp.area() / boxRect.area() > 0.20f) 
            {
                bret = false;
                break;
            }
        }
    }

    return bret;
}

bool checkRaoLuByHistory(cv::Mat &src, cv::Rect &historyRect, std::vector<cv::Rect> &cars)
{
    bool bret = false;

    cv::RotatedRect rRect_onlyCar;
    rRect_onlyCar.center.x = src.cols * 0.5f;
    rRect_onlyCar.center.y = src.rows * 0.75f;
    rRect_onlyCar.size.width = src.cols * 0.10f;
    rRect_onlyCar.size.height = src.rows * 0.10f;
    rRect_onlyCar.angle = 0;

    for(auto car : cars) 
    {
        cv::Rect resultRect(car.x, car.y, car.width, car.height);
        
        /*if((car.x + car.width / 2 > src.cols / 3) && (car.y + car.height / 2 > src.rows / 3) 
            && (car.x + car.width < 0.90f * src.cols))
        {
            bret = true;
            break;
        }
        if(car.width * car.height > 0.5f * src.cols * src.rows) 
        {
            bret = true;
            break;
        }*/
        if(IsRectIntersection(rRect_onlyCar, resultRect)) 
        {
            bret = true;
            break;
        }
    }

    return bret;
}

//bit0: 0 no car, 1: has car; bit1: 1:may raolu; bit2: 1:no detect; bit3 sure detect no car; bit4 empty park is wider; bit5 raolu car box is big;
//
int carInParkProcess(cv::Mat &srcImg, std::vector<YoloBox> &boxes, YoloBox &carBox)
{
    int hasCar = 0x04;
    bool isRaoLu = false;
    cv::Rect historyBox;
    float prob = 0.0;
    
    bool isCarBoxBig = false;

    char save[128];

    float xEdge = 0.92f * srcImg.cols;
    float yEdge = 0.95f * srcImg.rows;
    float maxHigh = 0.70f * srcImg.rows;
    float maxWidth = 0.78f * srcImg.cols;
    
    std::vector<cv::Rect> cars;

    memset(&historyBox, 0, sizeof(historyBox));

    cv::RotatedRect rRect_right;

    rRect_right.center.x = srcImg.cols * 33 / 36;//1760
    rRect_right.center.y = srcImg.rows * 32 / 36;//960
    rRect_right.size.width = 180;
    rRect_right.size.height = 10;
    rRect_right.angle = -80;

    //find car first
    for(auto box : boxes) {
        cv::Rect resultRect(box.x, box.y, box.w, box.h);
        //car
        if(box.id == 2 && !IsRectIntersection(rRect_right, resultRect)) {
            cars.push_back(resultRect);
        }

        if(box.id == 2)
        {
            if(1.0 * box.w * box.h / (1.0 * srcImg.cols * srcImg.rows) > 0.28)
            {
                isCarBoxBig = true;
            }
        }
    }

    for(auto box : boxes) {
        cv::Rect resultRect(box.x, box.y, box.w, box.h);
        //car in park
        if(box.id == 0) {
            if(cars.size() > 0) {
                if(isCarNotInPark(resultRect, box.prob, boxes)) {
                    //kylog->info("isCarNotInPark\r\n");
                    continue;
                }
            }
            if(!IsRectIntersection(rRect_right, resultRect)) {
                if(resultRect.x + resultRect.width > historyBox.x + historyBox.width) {
                    historyBox = resultRect;
                    hasCar = 1;
                    prob = box.prob;
                }
            }
            else if(checkRaoLu(resultRect, cars)) 
            {
                if(resultRect.x + resultRect.width > historyBox.x + historyBox.width) 
                {    
                    //kylog->info("checkRaoLu ok\r\n");
                    historyBox = resultRect;
                    hasCar = 1;
                    prob = box.prob;
                }
            }
        }

        //empty park
        if(box.id == 3) {
#ifndef TWO_WHEEL
            if(!IsRectIntersection(rRect_right, resultRect) && (resultRect.y + resultRect.height > historyBox.y + historyBox.height)
                    && resultRect.y + resultRect.height < srcImg.rows - 40) 
            {
#else
            if(!IsRectIntersection(rRect_right, resultRect)) {
#endif
                if(resultRect.x + resultRect.width > historyBox.x + historyBox.width) {
                    historyBox = resultRect;
                    hasCar = 0;
                    prob = box.prob;
                }
            }
        }

        //car in park, part of car
        if(box.id == 0 || box.id == 1) {
            if(resultRect.height > maxHigh && resultRect.width > maxWidth 
                    && resultRect.y < 70 && checkBigCarWithEmptyPark(boxes, resultRect)) 
            {
                //kylog->info("big car\r\n");
                historyBox = resultRect;
                hasCar = 1;
                prob = box.prob;    
                break;
            }
        }
        
    }

    //maybe raolu
    if(checkRaoLuByHistory(srcImg, historyBox, cars))
    {
        //kylog->info("may rao lu\r\n");
        isRaoLu = true;
    }

    if(hasCar > 0) 
    {
        if((historyBox.x + historyBox.width < 0.33f * srcImg.cols) && (historyBox.y + historyBox.height < 0.55f * srcImg.rows)) 
        {
            //kylog->info("too far\r\n");
            hasCar = 0;
        }
    }

    /** draw remark **/
    if(0)
    {
        for(auto box : boxes) {
            cv::Rect drawRect(box.x, box.y, box.w, box.h);
            
            switch(box.id)
            {
                case 0://car_in_park
                    cv::rectangle(srcImg, drawRect, cv::Scalar(0, 0, 255), 1, 1, 0);
                    break;
                case 1://part_of_car
                    cv::rectangle(srcImg, drawRect, cv::Scalar(0, 255, 0), 1, 1, 0);
                    break;
                case 2://car
                    cv::rectangle(srcImg, drawRect, cv::Scalar(255, 64, 32), 1, 1, 0);
                    break;
                case 3://empty_park
                    cv::rectangle(srcImg, drawRect, cv::Scalar(0, 128, 255), 1, 1, 0);
                    break;
                case 4://plate
                    cv::rectangle(srcImg, drawRect, cv::Scalar(0, 255, 255), 1, 1, 0);
                    break;
            }
        }
    }


    if(hasCar) 
    {
        //cv::rectangle(srcImg, historyBox, cv::Scalar(255, 128, 128), 3, 1, 0);
        carBox.x = historyBox.x;
        carBox.y = historyBox.y;
        carBox.w = historyBox.width;
        carBox.h = historyBox.height;
        carBox.id = 0;
        carBox.prob = prob;
    }
    else 
    {
        if(historyBox.x > 0 || historyBox.width > 0)
        {
            if((historyBox.x + historyBox.width < 0.36f * srcImg.cols) && (historyBox.y + historyBox.height < 0.55f * srcImg.rows)) 
            {
                kylog->info("empty park too far\r\n");
                printf("empty park too far\r\n");
            }
            else
            {
                hasCar = hasCar | 0x08;
            }

#ifndef TWO_WHEEL
            if(historyBox.width > srcImg.cols * 0.51 && historyBox.y + historyBox.height < 0.88 * srcImg.rows)
            {
                hasCar = hasCar | 0x10;
            }
#else 
            if(historyBox.width > srcImg.cols * 0.51 && historyBox.y + historyBox.height < 0.92 * srcImg.rows 
                    && historyBox.x + historyBox.width < 0.90 * srcImg.cols)
            {
                hasCar = hasCar | 0x10;
            }
#endif
        }
        
    }

    if(isRaoLu) 
    {
        hasCar |= 0x02;
    }

    if(isCarBoxBig)
    {
        hasCar |= 0x20;
    }

    cars.clear();

    return hasCar;
}

//return bit0~3: id
//bit4: position ok
//bit5: last 2 image
//bit6: newer 2 image
int findCross(cv::Mat &srcImg, std::vector<YoloBox> &boxes, cv::Rect &retRect)
{
    int id = -1, id1 = -1;
    int ret = 0;

    bool hasPartOfCar = false;
    cv::Rect bottomRect, bottomRect2rd;
    cv::Rect partOfCarRect;
    cv::Rect interRect;

    memset(&bottomRect, 0, sizeof(bottomRect));
    memset(&bottomRect2rd, 0, sizeof(bottomRect2rd));
    memset(&interRect, 0, sizeof(interRect));
    memset(&retRect, 0, sizeof(retRect));

    for(auto box : boxes)
    {
        cv::Rect tmpRect(box.x, box.y, box.w, box.h);
        if(box.id == 1)//part of car
        {
            hasPartOfCar = true;
            partOfCarRect = tmpRect;
            break;
        }

        if(box.id == 0)//car in park
        {
            if(tmpRect.y + tmpRect.height > bottomRect.y + bottomRect.height)
            {
                id = box.id;
                bottomRect2rd = bottomRect;
                bottomRect = tmpRect;
            }
            else if(tmpRect.y + tmpRect.height > bottomRect2rd.y + bottomRect2rd.height)
            {
                id1 = box.id;
                bottomRect2rd = tmpRect;
            }
        }
        else if(box.id == 3)//empty park
        {
            if(tmpRect.y + tmpRect.height > srcImg.rows * 0.945)
            {
                continue;
            }
            else if(tmpRect.y + tmpRect.height > bottomRect.y + bottomRect.height)
            {
                id = box.id;
                bottomRect2rd = bottomRect;
                bottomRect = tmpRect;
            }
            else if(tmpRect.y + tmpRect.height > bottomRect2rd.y + bottomRect2rd.height)
            {
                id1 = box.id;
                bottomRect2rd = tmpRect;
            }
        }
    }

    if(hasPartOfCar)
    {
        memset(&bottomRect, 0, sizeof(bottomRect));
        memset(&bottomRect2rd, 0, sizeof(bottomRect2rd));
        for(auto box : boxes)
        {
            cv::Rect tmpRect(box.x, box.y, box.w, box.h);
            cv::Rect rect = tmpRect & partOfCarRect;
            
            float rate = rect.area() / partOfCarRect.area();
            if(rate > 0.79f)
            {
                continue;
            }

            if(box.id == 0)//car in park
            {
                if(tmpRect.y + tmpRect.height > bottomRect.y + bottomRect.height)
                {
                    id = box.id;
                    bottomRect2rd = bottomRect;
                    bottomRect = tmpRect;
                }
                else if(tmpRect.y + tmpRect.height > bottomRect2rd.y + bottomRect2rd.height)
                {
                    id1 = box.id;
                    bottomRect2rd = tmpRect;
                }
            }
            else if(box.id == 3)//empty park
            {
                if(tmpRect.y + tmpRect.height > srcImg.rows * 0.945)
                {
                    continue;
                }
                else if(tmpRect.y + tmpRect.height > bottomRect.y + bottomRect.height)
                {
                    id = box.id;
                    bottomRect2rd = bottomRect;
                    bottomRect = tmpRect;
                }
                else if(tmpRect.y + tmpRect.height > bottomRect2rd.y + bottomRect2rd.height)
                {
                    id1 = box.id;
                    bottomRect2rd = tmpRect;
                }
            }
        }
    }

    if(bottomRect.y + bottomRect.height < 1030 && bottomRect.y + bottomRect.height > 800 
            && (bottomRect.x + bottomRect.width / 2) < 0.6 * srcImg.cols)
    {
        //position is ok
        // kylog->info("bottomRect");
        retRect = bottomRect;
        ret = id;
        ret |= 0x10;
        return ret;
    }
    else if(bottomRect.y + bottomRect.height >= 1030 
            && (bottomRect.x + bottomRect.width / 2) < 0.6 * srcImg.cols)
    {
        //get last two image
        // kylog->info("bottomRect");
        retRect = bottomRect;
        ret = id;
        ret |= 0x20;
        return ret;
    }
    else if(bottomRect.y + bottomRect.height <= 800
            && (bottomRect.x + bottomRect.width / 2) < 0.6 * srcImg.cols)
    {
        //get newer two image
        // kylog->info("bottomRect");
        retRect = bottomRect;
        ret = id;
        ret |= 0x40;
        return ret;
    }
    

    if(bottomRect.height !=0 && bottomRect2rd.height != 0)
    {
        interRect = bottomRect & bottomRect2rd;
        if(interRect.y + interRect.height < 1030 && interRect.y + interRect.height > 800 
            && (interRect.x + interRect.width / 2) < 0.6 * srcImg.cols)
        {
            //position is ok
            // kylog->info("bottomRect 2rd");
            retRect = bottomRect2rd;
            ret = id1;
            ret |= 0x10;
            return ret;
        }
        else if(interRect.y + interRect.height >= 1030 
            && (interRect.x + interRect.width / 2) < 0.6 * srcImg.cols)
        {
            //get last two image
            // kylog->info("bottomRect 2rd");
            retRect = bottomRect2rd;
            ret = id1;
            ret |= 0x20;
            return ret;
        }
        else if(interRect.y + interRect.height <= 800
                && (interRect.x + interRect.width / 2) < 0.6 * srcImg.cols)
        {
            //get newer two image
            // kylog->info("bottomRect 2rd");
            retRect = bottomRect2rd;
            ret = id1;
            ret |= 0x40;
            return ret;
        }
    }

    return ret;
}

//bit4: cross position ok
//bit5: car position ok
int findCrossXiefei(cv::Mat &srcImg, std::vector<YoloBox> &boxes, YoloBox &retBox)
{
    int ret = 0;

    bool isLeftCrossOk = false;
    bool isRightCrossOk = false;

    bool isCarBoxOk = false;

    for(auto box : boxes)
    {
        if(box.id == 0)
        {
            if(box.y + box.h > 350 && box.x > 100 && box.x < 600)
            {
                isLeftCrossOk = true;
            }
            else if(box.y + box.h > 350 && box.x > 1100 && box.x < 1800)
            {
                isRightCrossOk = true;
            }
        }
        else if(box.id == 1)
        {
            if(box.y + box.h > 350 && box.x > 250 && box.x < 600 && (box.x + box.w) > 1100 && (box.x + box.w) < 1600)
            {
                isCarBoxOk = true;
                memcpy(&retBox, &box, sizeof(retBox));
            }
        }
    }

    if(isLeftCrossOk && isRightCrossOk)
    {
        ret |= 0x10;
    }
    else if(isCarBoxOk)
    {
        ret |= 0x20;
    }

    return ret;
}

//bit0: 1 have car
int isXiefeiHasCar(cv::Mat &midImg, std::vector<YoloBox> &boxes, float arpha1, float arpha2, YoloBox &findbox)
{
    int ret = 0;
    YoloBox crossLeft, crossRight;
    

    memset(&crossLeft, 0, sizeof(crossLeft));
    memset(&crossRight, 0, sizeof(crossRight));
    crossLeft.x = 999999999;
    for(auto box : boxes)
    {
        if(box.id == 0)
        {
            if(box.y + box.h > 350 && box.x > 100 && box.x < 600)
            {
                if(box.x < crossLeft.x)
                {
                    memcpy(&crossLeft, &box, sizeof(crossLeft));
                }
            }
            else if(box.y + box.h > 350 && box.x > 1100 && box.x < 1800)
            {
                if(box.x > crossRight.x)
                {
                    memcpy(&crossRight, &box, sizeof(crossRight));
                }
            }
        }
    }

    RotatedRect rRect_right;
    float angle = 90.0f;
    if(arpha1 != 0.0f && arpha2 != 0.0f)
    {
        angle = (arpha1 + arpha2) / 2;
    }

    rRect_right.center.x = (crossLeft.x + crossRight.x) / 2;
    rRect_right.center.y = (crossLeft.y + crossRight.y) / 2;
    rRect_right.size.width = 2400;
    rRect_right.size.height = 10;
    rRect_right.angle = -(180.0 - angle);

    for(auto box : boxes)
    {
        if(box.id == 1)
        {
            Rect resultRect(box.x + 0.04f * box.w, box.y, box.w * 0.92, box.h);
            if(IsRectIntersection(rRect_right, resultRect))
            {
                memcpy(&findbox, &box, sizeof(findbox));
                ret = 1;
                break;
            }
        }
    }
    

    return ret;
}

int isUpImgHasCar(cv::Mat &upImg, std::vector<YoloBox> &boxes, YoloBox &carbox, YoloBox &upCarbox)
{
    int ret = 0;

    for(auto box : boxes)
    {
        if(box.id == 1)
        {
            kylog->info("box = {}, {}, {}, {}", box.x, box.y, box.w, box.h);
            if(box.x < carbox.x + 250 && box.x > carbox.x - 250 && 
                    box.w < carbox.w + 280 &&
                    box.w > carbox.w - 280 &&
                    box.y + box.h > upImg.rows / 3)
            {
                memcpy(&upCarbox, &box, sizeof(upCarbox));
                ret = 1;
                break;
            }
        }
    }

    return ret;
}
